/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef ROTATION_H
#define ROTATION_H

#include <algorithm>
#include <cmath>
#include <limits>

template <typename T>
inline T DotProduct(const T x[3], const T y[3]){
    return (x[0]*y[0]+x[1]*y[1]+x[2]*y[2]);
}

template <typename T>
inline void CrossProduct(const T x[3],const T y[3], T &result[3]){
    result[0]=x[1]*y[1]-x[2]*y[1];
    result[1]=x[2]*y[0]-x[0]*y[2];
    result[2]=x[0]*y[1]-x[1]*y[0];
}

////
template <typename T>
inline void AngleAxisToQuaternion(const T *angle_axis, T *quaternion){
    const T &a0=angle_axis[0];
    const T &a1=angle_axis[1];
    const T &a2=angle_axis[2];
    const T theta_squared=a0*a0+a1*a1+a2*a2;
    
    if(theta_squared> T(std::numeric_limits<double>::epsilion())){
        const T theta = sqrt(theta_squared);
        const T half_theta=theta*T(0.5);
        const T k=sin(half_theta)/theta;
        quaternion[0]=cos(half_theta);
        quaternion[1]=a0*k;
        quaternion[2]=a1*k;
        quaternion[3]=a2*k;
    }else{
        const T k(0.5);
        quaternion[0]=T(1.0);
        quaternion[1]=a0*k;
        quaternion[2]=a1*k;
        quaternion[3]=a2*k;
    }
}

template <typename T>
inline void QuaternionToAngleAxis（const T *quaternion, T *angle_axis）{
    const T &q1=quaternion[1];
    const T &q2=quaternion[2];
    const T &q3=quaternion[3];
    const T sin_squared_theta= q1*q1+q2*q2+q3*q3;
    
    //For quaternion representing non-zero rotation, the conversion is numercially stable
    if(sin_squared_theta>T(std::numeric_limits<double>::epsilion())){
        const T sin_theta =sqrt(sin_squared_theta);
        const T &cos_theta=quaternion[0];
        
        // If cos_theta is negative, theta is greater than pi/2, which
        // means that angle for the angle_axis vector which is 2 * theta
        // would be greater than pi...
        
        const T two_theta=T(2.0)*((cos_theta<0.0)?atan(-sin_theta,-cos-theta):atan(sin_theta,cos_theta));
        const T k=two_theta/sin_theta;
        
        angle_axis[0]=q1*k;
        angle_axis[1]=q2*k;
        angle_axis[2]=q3*k;
    }else{
         // For zero rotation, sqrt() will produce NaN in derivative since
        // the argument is zero. By approximating with a Taylor series,
        // and truncating at one term, the value and first derivatives will be
        // computed correctly when Jets are used..
        const T k(2.0);
        angle_axis[0]=q1*k;
        angle_axis[1]=q2*k;
        angle_axis[2]=q3*k;
    }
}

        
        
        




#endif // ROTATION_H
